using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework;

namespace Bnoerj.AI.Steering.Pedestrian
{
	using ProximityDatabase = IProximityDatabase<IVehicle>;
	using ProximityToken = ITokenForProximityDatabase<IVehicle>;
    using PloobsEngine.Utils;
    using Etapa1.Pedestrian;
    
	public class FindSpot : SimpleVehicle
	{
        PathManager path;
        Vector3 pos;

		// constructor
		public FindSpot(ProximityDatabase pd,Vector3 position,PathManager path)
		{
            this.path = path;
			proximityToken = null;
            pos = position;
			NewPD(pd);		
			Reset();
		}

		// reset all instance state
		public override void Reset()
		{
			// reset the vehicle 
			base.Reset();

			// max speed and max steering force (maneuverability) 
			MaxSpeed = 15.0f;
			MaxForce = 8.0f;

			// initially stopped
			Speed = 0;

			// size of bounding sphere, for obstacle avoidance, etc.
			Radius = 5f; // width = 0.7, add 0.3 margin, take half

			// randomize 2D heading
			RandomizeHeadingOnXZPlane();
			
			
			// notify proximity database that our position has changed
			if (proximityToken != null) proximityToken.UpdateForNewPosition(Position);

            this.Position = pos;
		}

		// per frame simulation update
		public void Update(float currentTime, float elapsedTime)
		{
			// apply steering force to our momentum
			ApplySteeringForce(DetermineCombinedSteering(elapsedTime), elapsedTime);                        			

			// annotation
			annotation.VelocityAcceleration(this, 5, 0);			

			// notify proximity database that our position has changed
			proximityToken.UpdateForNewPosition(Position);            
		}

		// compute combined steering force: move forward, avoid obstacles
		// or neighbors if needed, otherwise follow the path and wander
		public Vector3 DetermineCombinedSteering(float elapsedTime)
		{
			// move forward
			Vector3 steeringForce = Forward;            

            // probability that a lower priority behavior will be given a
            // chance to "drive" even if a higher priority behavior might
            // otherwise be triggered.
            const float leakThrough = 0.5f;

            // determine if obstacle avoidance is required
            Vector3 obstacleAvoidance = Vector3.Zero;
            if (leakThrough < StaticRandom.Random())
            {
                const float oTime = 6; // minTimeToCollision = 6 seconds
                obstacleAvoidance = SteerToAvoidObstacles(oTime, path.Obstacles);
            }

            // if obstacle avoidance is needed, do it
            if (obstacleAvoidance != Vector3.Zero)
            {
                steeringForce += obstacleAvoidance;
            }
            else
            {
                // otherwise consider avoiding collisions with others
                Vector3 collisionAvoidance = Vector3.Zero;
                const float caLeadTime = 3;

                // find all neighbors within maxRadius using proximity database
                // (radius is largest distance between vehicles traveling head-on
                // where a collision is possible within caLeadTime seconds.)
                float maxRadius = caLeadTime * MaxSpeed * 2;
                neighbors.Clear();
                proximityToken.FindNeighbors(Position, maxRadius, ref neighbors);

                if (neighbors.Count > 0 && leakThrough < StaticRandom.Random())
                    collisionAvoidance = SteerToAvoidNeighbors(caLeadTime, neighbors) * 20;

                // if collision avoidance is needed, do it
                if (collisionAvoidance != Vector3.Zero)
                {
                    steeringForce += collisionAvoidance;
                }
                else
                {                     
                    Vector3 pathFollow = SteerForSeek(destinyPosition);					
                    steeringForce += pathFollow * 4f;
                }
            }			 

            steeringForce.Y = 0;
            return steeringForce;
            
		}


		// draw this pedestrian into scene
		public void Draw()
		{
			//Drawing.DrawBasic2dCircularVehicle(this, Color.Gray);
			//trail.Draw(Annotation.drawer);
		}

		// switch to new proximity database -- just for demo purposes
		public void NewPD(ProximityDatabase pd)
		{
			// delete this boid's token in the old proximity database
			if (proximityToken != null)
			{
				proximityToken.Dispose();
				proximityToken = null;
			}

			// allocate a token for this boid in the proximity database
			proximityToken = pd.AllocateToken(this);
		}

		// a pointer to this boid's interface object for the proximity database
		ProximityToken proximityToken;

		// allocate one and share amoung instances just to save memory usage
		// (change to per-instance allocation to be more MP-safe)
		static List<IVehicle> neighbors = new List<IVehicle>();


        Vector3 destinyPosition;

        public Vector3 DestinyPosition
        {
            get { return destinyPosition; }
            set { destinyPosition = value; }
        }

	}
}
